//
// Created by ze on 2023/11/9.
//

#ifndef ENCODER_UPRE_PATH_H
#define ENCODER_UPRE_PATH_H
#include "UPRE_GLOBAL_DEFINE.h"

//定义路径点个数
#define UPRE_PATH_PATHNUM 4001
//计算每个路径点的增量
#define UPRE_PATH_DivideEqually (1.f/(UPRE_PATH_PATHNUM - 1))

extern point Path_PathPoint[UPRE_PATH_PATHNUM];
/*********************************函数声明***************************************/
bool StraigthLinePathInit(const StraightLinePath* path, bool isDefaultParam,
                          int32_t startSpeed, int32_t maxSpeed, int32_t endSpeed,
                          uint8_t acceleratePercent, uint8_t descentPercent, uint8_t yawAdjustPercent);
bool B_SpLinePathInit(const B_Spline_PathCfg* path, bool isDefaultParam,
                      int32_t startSpeed, int32_t maxSpeed, int32_t endSpeed,
                      uint8_t acceleratePercent, uint8_t descentPercent, uint8_t yawAdjustPercent);
bool PATH_B_SpLinePathInit_4ControlPoint(BsplinePath* bsplinePath, bool isDefaultParam,
                                         int32_t startSpeed, int32_t maxSpeed, int32_t endSpeed,
                                         uint8_t acceleratePercent, uint8_t descentPercent, uint8_t yawAdjustPercent);
bool Path_BesselPathInit(PATH_BesselControlPoint *Path_BesselControlPoint, bool isDefaultParam,
                         int32_t startSpeed, int32_t maxSpeed, int32_t endSpeed,
                         uint8_t acceleratePercent, uint8_t descentPercent, uint8_t yawAdjustPercent);
PATH_STATE goPath();
void setPathVel(float velSize, float pathVelDir);
vector getPathVel(void);
PATH_STATE getPathState();
PathParam pathconfig(void);
float optimizeRobRotation(float targetYaw);
extern Pose Print_Stright_Pose;
extern float path_percentNow;
extern uint8_t path_PathState;
extern uint8_t path_type;
extern Pose Print_End_Pose;
#endif //ENCODER_UPRE_PATH_H
